#include "DAP_config.h"
#include "DAP.h"
#include "MULT.h"
#include "MULT_I2C.h"

static uint32_t mult_mode = MULT_MODE_DEFAULT;

static const mult_interface_t *mult_interface[MULT_MODE_NUMBER] = {
    &mult_interface_i2c,
};

static const uint8_t mult_info[] = {
	((MULT_ENABLE_I2C) << 0) | ((MULT_ENABLE_SPI) << 1),
};

static uint32_t MULT_ProcessCommand(const uint8_t *request,
                                    uint8_t *response)
{
    uint32_t num = (1U << 16) | 1U;

    *response++ = *request; // byte0 复制命令ID

    switch (*request++)
    {
    case ID_MULT_Info: // > 0x80 + info_ID(1b)
                       // < 0x80 + info(1b)
        num += ((1U << 16) | 1U);
		*response = mult_info[*request];
        break;
    case ID_MULT_Setup: // > 0x81 + mode_ID(1b)
                        // < 0x81 + DAP_OK(1b)
        if (*request < MULT_MODE_NUMBER)
        {
            *response = DAP_OK;
            mult_interface[*request]->mult_init();
        }
        else
        {
			PORT_OFF();
            *response = DAP_ERROR;
        }
        num += ((1U << 16) | 1U); // 消耗1，回复1
        break;
    case ID_MULT_Cmd: // > 0x83 + cmd_ID(1b) + value(1b)
                      // < 0x83 + DAP_OK(1b) + value(1b)
        num += mult_interface[mult_mode]->mult_cmd(request, response);
        break;
    case ID_MULT_Read: // > 0x84 + number(1b)
                       // < 0x84 + number(1b) + data...
        num += mult_interface[mult_mode]->mult_read(request, response);
        break;
    case ID_MULT_Write: // > 0x85 + number(1b) + data...
                        // < 0x85 + number(1b)
        num += mult_interface[mult_mode]->mult_write(request, response);
        break;
    default:
        *(response - 1) = ID_DAP_Invalid; // 返回错误CMD
        return 0;
    }

    return (num);
}

/**
 * @brief 执行命令
 *
 * @param request 发来的命令
 * @param response 回复的命令
 * @return uint32_t 高16位是消耗的数据长度，低16位是回复的数据长度
 */
uint32_t DAP_ProcessVendorCommand(const uint8_t *request,
                                  uint8_t *response)
{
    // 包含多条命令
    if (*request == ID_MULT_Queue) // > 0x82 + number(1b) + ...
    {                              // < 0x82 + number(1b) + ...
        uint32_t cnt = 0;
        uint32_t num = (2U << 16) | 2U; // ID + cmd_number
        uint32_t n;                     //

        *response++ = *request++;               // 复制ID
        const uint32_t cmd_number = *request++; // 命令条数
        uint8_t *cmd_number_return = response;
        // *response++ = (uint8_t)cmd_number;

        while (cnt < cmd_number)
        {
            n = MULT_ProcessCommand(request, response);
            if (n == 0) // 不存在的指令
            {
                break;
            }
            num += n;
            request += (uint16_t)(n >> 16);
            response += (uint16_t)n;

            cnt++;
        }
        *cmd_number_return = (uint8_t)cnt;
        return (num);
    }

    return MULT_ProcessCommand(request, response);
}
